// file: ec.cc
//

// system include files
//
#include <memory.h>
#include <math.h>

// local include files
//
#include "ec.h"
#include "cb.h"
#include "ec_constants.h"

//-----------------------------------------------------------------------------
//
// public methods
//
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
//
// method: init_cc
// description: initialization
//
// arguments: all user-controlled parameters related to the algorithm
//
// return: none
//
// this method initializes all internal parameters
//
//-----------------------------------------------------------------------------
void Echo_canceller::init_cc(double gamma, int N, int M, double beta1,
			     double sigma_ly, double sigma_lu,
			     double alpha_st, double alpha_yt, double cutoff,
			     int hangt, double suppr, double tau) {

  // reset parameters
  //
  gamma_d = gamma;
  N_d = N;
  M_d = M;
  beta1_d = beta1;
  beta2_d = beta1;
  tau_d = tau;
  start_speech_d = 0;
  flag_d = 1;
  sigma_Ly_d = sigma_ly;
  sigma_Lu_d = sigma_lu;
  alpha_st_d = alpha_st;
  alpha_yt_d = alpha_yt;
  CUTOFF_d = cutoff;
  HANGT_d = hangt;
  SUPPR_d = suppr;
  
  // reset the high-pass filter
  //
  s_i_d = 0.0;
  sdc_d = 0.0;
  sdc_im1_d = 0.0;

  // reset the circular buffers
  //
  y_d.allocate_cc(N_d + M_d);
  s_d.allocate_cc(N_d + M_d);
  u_d.allocate_cc(M_d);
  e_d.allocate_cc(M_d);

  // allocate a buffer for the reference signal power computation
  //
  y_tilde_d.allocate_cc(N_d);

  // allocate coefficient memory
  //
  if (a_d != (double*)NULL)
    {delete [] a_d;}
  a_d = new double[N_d];
  memset(a_d, (int)0, sizeof(double)*N_d);

  // reset absolute time
  //
  i_d = (int)0;
  
  // reset the power computations (for y and u)
  //
  Ly_d = CUTOFF_d;
  Lu_d = 0.0;

  // reset the near-end speech detector
  //
  s_tilde_d = 0.0;
  HCNTR_d = (int)0;

  // exit gracefully
  //
}

//-----------------------------------------------------------------------------
//
// method: process_cc
// description: processes one sample of data through the echo canceller
//
// arguments:
//  double ref: reference signal (outgoing speech)
//  double sig: incoming signal (signal plus echo)
//
// return:
//  a double value containing the echo canceller output
//
//-----------------------------------------------------------------------------
double Echo_canceller::process_cc(double ref, double sig) {

  // declare local variables that are used more than once
  //
  int k;

  //***************************************************************************
  //
  // flow A on pg. 428
  //
  //***************************************************************************

  // eq. (16): high-pass filter the input to generate the next value;
  //           push the current value into the circular buffer
  //
  // sdc_im1_d = sdc_d;
  // sdc_d = sig;
  //  s_i_d = sdc_d;
  //  s_d = s_i_d;
  //  s_i_d = (double)(1.0 - gamma_d) * s_i_d
  //    + (double)(0.5 * (1.0 - gamma_d)) * (sdc_d - sdc_im1_d);
  
  // push the reference data onto the circular buffer
  //
  y_d = ref;
  s_d = sig;
  
  // eq. (2): compute r
  //
  double r = 0;
  
  for(k=0; k<N_d; k++) {
    r += a_d[k] * (double)y_d(-k);
  }
  
  // eq. (3): compute the output value (see figure 3) and the error
  // note: the error is the same as the output signal when near-end
  // speech is not present
  //
  double u_suppr = (double)s_d - r;
  
  e_d = u_d = u_suppr;
  
  // eq. (12) and (13): update near-end speech detector
  //
  s_tilde_d = (1.0 - alpha_st_d) * s_tilde_d 
    + alpha_st_d * fabs((double)s_d);
  y_tilde_d = (1.0 - alpha_yt_d) * (double)y_tilde_d 
    + alpha_st_d * fabs((double)y_d);
  
  //***************************************************************************
  //
  // flow B on pg. 428
  //
  //***************************************************************************
  
  // compute the new convergence factor
  //
  double Py = Ly_d * Ly_d;
  if (HCNTR_d > 0){
    Py = 1.0;
  }
  
  // Vary rate of adaptation depending on position in the file
  // Do not do this for the first (DEFAULT_UPDATE_TIME) secs after speech
  // has begun of the file to allow the echo cancellor to estimate the
  // channel accurately
  //
  if ( start_speech_d != 0 ){
    if ( i_d > (DEFAULT_T0 + start_speech_d)*(SAMPLE_FREQ) ){
      beta2_d = max_cc(MIN_BETA,
		       beta1_d * exp((-1/tau_d)*((i_d/(double)SAMPLE_FREQ) -
						 DEFAULT_T0 -
						 start_speech_d)));
    }
  }
  else {beta2_d = beta1_d;}
  
  double two_beta = beta2_d / Py;
  if (two_beta > MAX_BETA)
    {two_beta = MAX_BETA;}
  
  // eq. (15): update power estimate of the return signal power
  //
  Lu_d = (1.0 - sigma_Lu_d) * Lu_d + sigma_Lu_d * fabs((double)u_d);
  
  // eq. (10): update power estimate of the reference
  //
  Ly_d = (1.0 - sigma_Ly_d) * Ly_d + sigma_Ly_d * fabs((double)y_d);
  if (Ly_d < CUTOFF_d)
    {Ly_d = CUTOFF_d;}
  
  // eq. (14): is there near-end speech?
  //
  double max_y_tilde = 0;
  for (k=0; k<N_d; k++) {
    if (max_y_tilde < y_tilde_d(-k))
      {max_y_tilde = y_tilde_d(-k);}
  }

  if (s_tilde_d > (0.2*max_y_tilde))
    {
      HCNTR_d = HANGT_d;
    }
  else if (HCNTR_d > (int)0)
    {
      HCNTR_d--;
    }

  // update coefficients if no near-end speech
  //
  if ((HCNTR_d == 0) && (i_d % M_d == (int)0)){
    
    // loop over all filter coefficients
    //
    for (k=0; k<N_d; k++) {
      
      // eq. (7): compute an expectation over M_d samples 
      //
      double grad = 0.0;
      
      for (int m=0; m<M_d; m++)
	{grad += (double)e_d(-m) * (double)y_d(-m-k);}
      
      // eq. (7): update the coefficient
      //
      a_d[k] += two_beta * grad;
    }
  }

  if ((flag_d == 1) && ((Ly_d/Lu_d) < pow((double)10.0,(ONSET_THRESH/10.0)))) {
    start_speech_d = i_d/8000.0;
    flag_d = 0;
  }

  // paragraph below eq. (15): if no near-end speech,
  // check for residual error suppression
  //
  float  suppr_value = pow(10,SUPPR_d/10.0);

  if ((HCNTR_d == 0) && ((Lu_d/Ly_d) < suppr_value) &&
      ((10*log(Lu_d/Ly_d)) > EC_MIN_DB_VALUE)) {
    
    float suppr_factor = (1/(float)(SUPPR_FLOOR-SUPPR_CEIL))*10*log(Lu_d/Ly_d)
      - SUPPR_CEIL/(float)(SUPPR_FLOOR - SUPPR_CEIL);

    u_suppr = pow(10.0,(suppr_factor)*RES_SUPR_FACTOR/10.0)*u_suppr;
    
  }
  
  // exit gracefully
  //
  i_d++;
  return u_suppr;
}

//-----------------------------------------------------------------------------
// method: constructor
// description: creates an echo canceller object and initializes all buffers
//
//-----------------------------------------------------------------------------
Echo_canceller::Echo_canceller() {

  // initialize coefficient memory
  //
  a_d = (double*)NULL;

  // exit gracefully
  //
}

Echo_canceller::~Echo_canceller() {
  
  // free coefficient memory
  //
  delete [] a_d;

  // exit gracefully
  //
}

//-----------------------------------------------------------------------------
//
// method: max_cc
// description: returns the maximum of two numbers (I hate macros!)
//
//-----------------------------------------------------------------------------
int Echo_canceller::max_cc(int x, int y) {
  if (x > y)
    {return x;}
  else
    {return y;}
}

double Echo_canceller::max_cc(double x, double y) {
  if (x > y)
    {return x;}
  else
    {return y;}
}


//-----------------------------------------------------------------------------
//
// method: clip_cc
// description: clips a sample value to +/-MAX_AMPL
//
//-----------------------------------------------------------------------------
short int Echo_canceller::clip_cc(double value) {

  // restore the range of value
  //
  value *= AMPL_SCALE_2;

  // clip it to +/-AMPL_SCALE_2
  //
  if (value > AMPL_SCALE_2)
    {return (short int)AMPL_SCALE_2;}
  else if (value < -AMPL_SCALE_2)
    {return (short int)-AMPL_SCALE_2;}
  else
    //{return (short int)rint(value);}
    {return (short int)(value);}
}
